/***************************************************************************\
 * Copyright (C) by Keio University
 * OpenNISensor.cpp created in 07 2012.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * OpenNISensor.cpp is part of HVRL Engine Library.
 *
 * The HVRL Engine Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/sensor/OpenNISensor.hpp"
#include "hvrl/common/Common.hpp"

#if (XN_PLATFORM == XN_PLATFORM_LINUX_X86 || XN_PLATFORM == XN_PLATFORM_LINUX_ARM)
#define UNIX
#define GLX_GLXEXT_LEGACY
#endif

#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
#define MACOS
#endif

#include <XnLog.h>
#include <XnTypes.h>


namespace hvrl {

OpenNISensor::OpenNISensor(const unsigned int & id,
		const hvrl::OpenNISensorDriver& driver) :
		Sensor(), paused(false), grabcolor(true), running(false), finished(true), updatepc(false), pthread(0){

	this->context = driver.getContext();
	this->device_id = id;

	XnStatus nRetVal = XN_STATUS_OK;

	unsigned int i = 0;
	bool created = false;
	xn::NodeInfoList list;
	xn::EnumerationErrors errors;
	nRetVal = context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list,
			&errors);
	for (xn::NodeInfoList::Iterator it = list.Begin();
			it != list.End() && created == false; ++it, ++i) {
		if (id == i) {
			std::string idstring = hvrl::numberToString(id);
			this->devicename = driver.getName() + " (" + idstring.c_str() + ")";
			xn::NodeInfo deviceNodeInfo = *it;
			context.CreateProductionTree(deviceNodeInfo, this->device);
			xn::Query query;
			query.AddNeededNode(deviceNodeInfo.GetInstanceName());

			nRetVal = this->depth.Create(context, &query);
			if (nRetVal != XN_STATUS_OK) {
				throw hvrl::Exception("OpenNISensor::OpenNISensor",
						"Failed creating depth node: "
								+ std::string(xnGetStatusString(nRetVal)));
			}
			if (this->grabcolor == true) {
				nRetVal = this->color.Create(context, &query);
				if (nRetVal != XN_STATUS_OK) {
					throw hvrl::Exception("OpenNISensor::OpenNISensor",
							"Failed creating color node: "
									+ std::string(xnGetStatusString(nRetVal)));
				}
			} else {
				nRetVal = this->ir.Create(context, &query);
				if (nRetVal != XN_STATUS_OK) {
					throw hvrl::Exception("OpenNISensor::OpenNISensor",
							"Failed creating infrared node: "
									+ std::string(xnGetStatusString(nRetVal)));
				}
			}

			outputMode.nXRes = XN_VGA_X_RES;
			outputMode.nYRes = XN_VGA_Y_RES;
			outputMode.nFPS = 30;

			this->depth.SetMapOutputMode(outputMode);
			this->depth.GetMetaData(depthmetadata);
			int widthdepth = depthmetadata.FullXRes();
			int heightdepth = depthmetadata.FullYRes();

			rgbd.depth.setSize(Size(widthdepth,heightdepth));
			rgbd.depth.setMax(this->depth.GetDeviceMaxDepth());
			this->pc.setSize(Size(widthdepth,heightdepth));

			this->color.SetMapOutputMode(outputMode);
			this->color.GetMetaData(colormetadata);
			int widthcolor = colormetadata.FullXRes();
			int heightcolor = colormetadata.FullYRes();

			rgbd.color.setSize(Size(widthcolor,heightcolor));

			if (this->depth.IsCapabilitySupported(
					XN_CAPABILITY_ALTERNATIVE_VIEW_POINT)) {
				this->depth.GetAlternativeViewPointCap().SetViewPoint(
						this->color);
			}
			this->color.GetAlternativeViewPointCap().SetViewPoint(this->color);
			XnUInt64 F;
			XnDouble pixel_size;

			// get the focal length in mm (ZPD = zero plane distance)
			nRetVal = this->depth.GetIntProperty("ZPD", F);
			if (nRetVal != XN_STATUS_OK) {
				throw hvrl::Exception("OpenNISensor::OpenNISensor",
						"Failed getting ZPD property: "
								+ std::string(xnGetStatusString(nRetVal)));
			}

			// get the pixel size in mm ("ZPPS" = pixel size at zero plane)
			nRetVal = this->depth.GetRealProperty("ZPPS", pixel_size);
			if (nRetVal != XN_STATUS_OK) {
				throw hvrl::Exception("OpenNISensor::OpenNISensor",
						"Failed getting ZPPS property: "
								+ std::string(xnGetStatusString(nRetVal)));
			}

			// normal size comes from SXGA resolution
			pixel_size *= 2.0;

			// Compute the back-projection parameters assuming pixels are squared
			this->bpcoeff = pixel_size / float(F);

			created = true;

			signal_ready();
		}
	}

	if (created == false) {
		throw hvrl::Exception("OpenNISensor::OpenNISensor",
				"Failed finding the device " + hvrl::numberToString(id));
	}

	pthread = new boost::thread(&OpenNISensor::run,this);

}

OpenNISensor::~OpenNISensor(void) {

	this->stop();
	while (false == this->finished) {
		usleep(100);
	}

}

void OpenNISensor::pause(void) {
	this->paused = !this->paused;
}

void OpenNISensor::play(void) {
	this->paused = false;
	if (false == this->isRunning()) {
		this->pthread->join();
	}
}

void OpenNISensor::stop(void) {
	if (true == this->isRunning()) {
		this->running = false;
	}
}

void OpenNISensor::run(void) {

	XnStatus rc;
		this->color.StartGenerating();
	this->depth.StartGenerating();

	this->finished = false;

	this->running = true;

	this->updatepc = false;

	while (true == this->running) {
		if (false == this->paused) {
			/*
			 * Grab images from the device
			 */

			if (this->depth.IsNewDataAvailable()) {
				this->depth.WaitAndUpdateData();
			}
			if (true == this->grabcolor) {
				if (this->color.IsNewDataAvailable()) {
					this->color.WaitAndUpdateData();
				}
			} else {
				if (this->ir.IsNewDataAvailable()) {
					this->ir.WaitAndUpdateData();
				}
			}

			/*
			 * Update the RGBD image
			 */
			{
				boost::mutex::scoped_lock l(mutex);
				rgbd.color.setData(this->color.GetImageMap());
			}

			{
				boost::mutex::scoped_lock l(mutex);
				rgbd.depth.setData(this->depth.GetDepthMap());
			}

			this->updatepc = true;

			/*
			 * Emit a message to confirm the update
			 */
			signal_updated();
		}
		boost::this_thread::sleep( boost::posix_time::milliseconds(40000) );
	}

	if (true == this->grabcolor) {
		rc = this->color.StopGenerating();
		if (rc != XN_STATUS_OK) {
			hvrl::Log::add().error("OpenNISensor::run",
					"Error while stopping color generator: "
							+ std::string(xnGetStatusString(rc)));
		}
	} else {
		rc = this->ir.StopGenerating();
		if (rc != XN_STATUS_OK) {
			hvrl::Log::add().error("OpenNISensor::run",
					"Error while stopping ir generator: "
							+ std::string(xnGetStatusString(rc)));
		}
	}
	rc = this->depth.StopGenerating();
	if (rc != XN_STATUS_OK) {
		hvrl::Log::add().error("OpenNISensor::run",
				"Error while stopping depth generator: "
						+ std::string(xnGetStatusString(rc)));
	}
	this->finished = true;

}

bool OpenNISensor::getValuef(const std::string& name, float& value) {
	if ("bpc" == name) {
		value = this->bpcoeff;
		return true;
	}
	if ("widthdepth" == name) {
		value = float(this->outputMode.nXRes);
		return true;
	}
	if ("heightdepth" == name) {
		value = float(this->outputMode.nYRes);
		return true;
	}
	if ("widthcolor" == name) {
		value = float(this->outputMode.nXRes);
		return true;
	}
	if ("heightcolor" == name) {
		value = float(this->outputMode.nYRes);
		return true;
	}
	return false;
}

PointCloud * OpenNISensor::getPointCloud(void){

	if(this->updatepc){
		XnPoint3D p3dinput;
					XnPoint3D p3doutput;

					float *temppointcloud = this->pc.getData();
					for(unsigned int i = 0; i < this->outputMode.nYRes; ++i){
						for(unsigned int j = 0; j < this->outputMode.nXRes; ++j){
							p3dinput.X = this->outputMode.nYRes - j;
							p3dinput.Y = this->outputMode.nXRes - i;
							p3dinput.Z = this->depth.GetDepthMap()[i*this->outputMode.nXRes+j];
							this->depth.ConvertProjectiveToRealWorld(1,&p3dinput,&p3doutput);
							temppointcloud[(i*this->outputMode.nXRes+j)*4] = p3doutput.X*0.001;
							temppointcloud[(i*this->outputMode.nXRes+j)*4+1] = p3doutput.Y*0.001;
							temppointcloud[(i*this->outputMode.nXRes+j)*4+2] = p3doutput.Z*0.001;
							temppointcloud[(i*this->outputMode.nXRes+j)*4+3] = 1.0f;
							if(p3doutput.Z==0){
								temppointcloud[(i*this->outputMode.nXRes+j)*4+3] = 0.0f;
							}else{
								temppointcloud[(i*this->outputMode.nXRes+j)*4+3] = 1.0f;
							}
						}
					}
		this->updatepc = false;
	}
	return &(this->pc);

}

}
